import { PoolManager } from '../managers/PoolManager';
import { cBody } from './Body';
import { ShapeSupport } from './Shape';
import { Vector2, Vector3 } from '../utils/Math';
import { PhysicsObject, Dirty } from './PhysicsObject';
import { Octree, AABB3D } from './Octree';

// ================== 2D AABB ==================
export class AABB2D {
    min: Vector2;
    max: Vector2;
    constructor(min: Vector2, max: Vector2) {
        this.min = min;
        this.max = max;
    }
    static intersects(a: AABB2D, b: AABB2D): boolean {
        return a.max.x >= b.min.x && a.min.x <= b.max.x &&
               a.max.y >= b.min.y && a.min.y <= b.max.y;
    }
    static fromPoints(points: Vector2[]): AABB2D {
        let minX = points[0].x, minY = points[0].y, maxX = points[0].x, maxY = points[0].y;
        for (const p of points) {
            if (p.x < minX) minX = p.x;
            if (p.x > maxX) maxX = p.x;
            if (p.y < minY) minY = p.y;
            if (p.y > maxY) maxY = p.y;
        }
        return new AABB2D(new Vector2(minX, minY), new Vector2(maxX, maxY));
    }
}

// ================== 2D Quadtree ==================
export class QuadtreeNode2D {
    bounds: { x: number, y: number, w: number, h: number };
    objects: { position: Vector2, radius: number, id: number }[] = [];
    children: QuadtreeNode2D[] | null = null;
    maxObjects: number;
    maxDepth: number;
    depth: number;
    constructor(bounds: { x: number, y: number, w: number, h: number }, maxObjects = 8, maxDepth = 6, depth = 0) {
        this.bounds = bounds;
        this.maxObjects = maxObjects;
        this.maxDepth = maxDepth;
        this.depth = depth;
    }
    insert(obj: { position: Vector2, radius: number, id: number }) {
        if (this.children) {
            const idx = this.getChildIndex(obj.position);
            this.children[idx].insert(obj);
            return;
        }
        this.objects.push(obj);
        if (this.objects.length > this.maxObjects && this.depth < this.maxDepth) {
            this.subdivide();
            for (const o of this.objects) {
                const idx = this.getChildIndex(o.position);
                this.children![idx].insert(o);
            }
            this.objects = [];
        }
    }
    queryRange(center: Vector2, range: number, result: number[] = []) {
        if (!this.intersects(center, range)) return result;
        for (const o of this.objects) {
            if (o.position.distance(center) <= range + o.radius) {
                result.push(o.id);
            }
        }
        if (this.children) {
            for (const child of this.children) {
                child.queryRange(center, range, result);
            }
        }
        return result;
    }
    private intersects(center: Vector2, range: number): boolean {
        const { x, y, w, h } = this.bounds;
        let dx = Math.max(Math.abs(center.x - x) - w / 2, 0);
        let dy = Math.max(Math.abs(center.y - y) - h / 2, 0);
        return dx * dx + dy * dy <= range * range;
    }
    private getChildIndex(pos: Vector2): number {
        const { x, y } = this.bounds;
        let idx = 0;
        if (pos.x >= x) idx |= 1;
        if (pos.y >= y) idx |= 2;
        return idx;
    }
    private subdivide() {
        this.children = [];
        const { x, y, w, h } = this.bounds;
        const hw = w / 2, hh = h / 2;
        this.children.push(new QuadtreeNode2D({ x: x - hw / 2, y: y - hh / 2, w: hw, h: hh }, this.maxObjects, this.maxDepth, this.depth + 1));
        this.children.push(new QuadtreeNode2D({ x: x + hw / 2, y: y - hh / 2, w: hw, h: hh }, this.maxObjects, this.maxDepth, this.depth + 1));
        this.children.push(new QuadtreeNode2D({ x: x - hw / 2, y: y + hh / 2, w: hw, h: hh }, this.maxObjects, this.maxDepth, this.depth + 1));
        this.children.push(new QuadtreeNode2D({ x: x + hw / 2, y: y + hh / 2, w: hw, h: hh }, this.maxObjects, this.maxDepth, this.depth + 1));
    }
}

// ================== 统一碰撞检测类 ==================
export class Collision {
    // ===== 2D 静态方法 =====
    /**
     * 2D圆形碰撞检测
     * @param a 圆A中心
     * @param r1 圆A半径
     * @param b 圆B中心
     * @param r2 圆B半径
     * @returns 是否碰撞
     * @example
     *   Collision.circleCircle(new Vector2(0,0), 1, new Vector2(1,0), 1) // true
     */
    static circleCircle(a: Vector2, r1: number, b: Vector2, r2: number): boolean {
        const distSq = a.distanceSq(b);
        const r = r1 + r2;
        return distSq <= r * r;
    }
    /**
     * 2D矩形（AABB）碰撞检测
     * @param a 矩形A中心
     * @param aw A宽
     * @param ah A高
     * @param b 矩形B中心
     * @param bw B宽
     * @param bh B高
     * @returns 是否碰撞
     */
    static rectRect(a: Vector2, aw: number, ah: number, b: Vector2, bw: number, bh: number): boolean {
        return Math.abs(a.x - b.x) * 2 < (aw + bw) && Math.abs(a.y - b.y) * 2 < (ah + bh);
    }
    /**
     * 2D多边形碰撞检测（支持凸多边形）
     * @param polyA 多边形A顶点数组
     * @param polyB 多边形B顶点数组
     * @returns 是否碰撞
     */
    static polygonPolygon(polyA: Vector2[], polyB: Vector2[]): boolean {
        const aabbA = AABB2D.fromPoints(polyA);
        const aabbB = AABB2D.fromPoints(polyB);
        if (!AABB2D.intersects(aabbA, aabbB)) return false;
        function getAxes(poly: Vector2[]): Vector2[] {
            const axes: Vector2[] = [];
            for (let i = 0; i < poly.length; i++) {
                const p1 = poly[i];
                const p2 = poly[(i + 1) % poly.length];
                const edge = p2.sub(p1);
                const normal = new Vector2(-edge.y, edge.x).normalize();
                axes.push(normal);
            }
            return axes;
        }
        function project(poly: Vector2[], axis: Vector2): [number, number] {
            let min = poly[0].dot(axis), max = min;
            for (let i = 1; i < poly.length; i++) {
                const p = poly[i].dot(axis);
                if (p < min) min = p;
                if (p > max) max = p;
            }
            return [min, max];
        }
        const axes = [...getAxes(polyA), ...getAxes(polyB)];
        for (const axis of axes) {
            const [minA, maxA] = project(polyA, axis);
            const [minB, maxB] = project(polyB, axis);
            if (maxA < minB || maxB < minA) return false;
        }
        return true;
    }
    /**
     * 2D圆与多边形碰撞检测
     * @param circle 圆心
     * @param radius 半径
     * @param poly 多边形顶点数组
     * @returns 是否碰撞
     */
    static circlePolygon(circle: Vector2, radius: number, poly: Vector2[]): boolean {
        const aabb = AABB2D.fromPoints(poly);
        const circleAABB = new AABB2D(
            new Vector2(circle.x - radius, circle.y - radius),
            new Vector2(circle.x + radius, circle.y + radius)
        );
        if (!AABB2D.intersects(aabb, circleAABB)) return false;
        for (let i = 0; i < poly.length; i++) {
            const p1 = poly[i];
            const p2 = poly[(i + 1) % poly.length];
            const edge = p2.sub(p1);
            const axis = new Vector2(-edge.y, edge.x).normalize();
            let minP = poly[0].dot(axis), maxP = minP;
            for (let j = 1; j < poly.length; j++) {
                const proj = poly[j].dot(axis);
                if (proj < minP) minP = proj;
                if (proj > maxP) maxP = proj;
            }
            const projCenter = circle.dot(axis);
            const minC = projCenter - radius;
            const maxC = projCenter + radius;
            if (maxP < minC || maxC < minP) return false;
        }
        let minDist = Infinity;
        let closest: Vector2 = poly[0];
        for (const v of poly) {
            const dist = v.distanceSq(circle);
            if (dist < minDist) {
                minDist = dist;
                closest = v;
            }
        }
        const axis = closest.sub(circle).normalize();
        let minP = poly[0].dot(axis), maxP = minP;
        for (let j = 1; j < poly.length; j++) {
            const proj = poly[j].dot(axis);
            if (proj < minP) minP = proj;
            if (proj > maxP) maxP = proj;
        }
        const projCenter = circle.dot(axis);
        const minC = projCenter - radius;
        const maxC = projCenter + radius;
        if (maxP < minC || maxC < minP) return false;
        return true;
    }
    /**
     * 2D批量圆碰撞检测
     * @param positions 所有圆心
     * @param radii 所有半径
     * @returns 所有碰撞对下标
     */
    static detectAllCircleCollisions(positions: Vector2[], radii: number[]): [number, number][] {
        const result: [number, number][] = [];
        for (let i = 0; i < positions.length; i++) {
            for (let j = i + 1; j < positions.length; j++) {
                if (this.circleCircle(positions[i], radii[i], positions[j], radii[j])) {
                    result.push([i, j]);
                }
            }
        }
        return result;
    }
    /**
     * 2D网格加速批量圆碰撞检测
     * @param positions 所有圆心
     * @param radii 所有半径
     * @param cellSize 网格单元大小
     * @returns 所有碰撞对下标
     */
    static detectAllCircleCollisionsGrid(positions: Vector2[], radii: number[], cellSize: number): [number, number][] {
        const grid = new Map<string, number[]>();
        for (let i = 0; i < positions.length; i++) {
            const cellX = Math.floor(positions[i].x / cellSize);
            const cellY = Math.floor(positions[i].y / cellSize);
            const key = `${cellX},${cellY}`;
            if (!grid.has(key)) grid.set(key, []);
            grid.get(key)!.push(i);
        }
        const checked = new Set<string>();
        const result: [number, number][] = [];
        for (const [key, indices] of grid.entries()) {
            const [cellX, cellY] = key.split(',').map(Number);
            for (let dx = -1; dx <= 1; dx++) {
                for (let dy = -1; dy <= 1; dy++) {
                    const neighborKey = `${cellX + dx},${cellY + dy}`;
                    if (!grid.has(neighborKey)) continue;
                    const pairKey = key < neighborKey ? `${key}|${neighborKey}` : `${neighborKey}|${key}`;
                    if (checked.has(pairKey)) continue;
                    checked.add(pairKey);
                    const indices2 = grid.get(neighborKey)!;
                    for (const i of indices) {
                        for (const j of indices2) {
                            if (i < j && this.circleCircle(positions[i], radii[i], positions[j], radii[j])) {
                                result.push([i, j]);
                            }
                        }
                    }
                }
            }
        }
        return result;
    }
    /**
     * 2D四叉树加速批量圆碰撞检测
     * @param positions 所有圆心
     * @param radii 所有半径
     * @param bounds 四叉树空间边界
     * @param maxObjects 单节点最大对象数
     * @param maxDepth 最大深度
     * @returns 所有碰撞对下标
     */
    static detectAllCircleCollisionsQuadtree(
        positions: Vector2[],
        radii: number[],
        bounds: { x: number, y: number, w: number, h: number },
        maxObjects = 8,
        maxDepth = 6
    ): [number, number][] {
        const qt = new QuadtreeNode2D(bounds, maxObjects, maxDepth);
        for (let i = 0; i < positions.length; i++) {
            qt.insert({ position: positions[i], radius: radii[i], id: i });
        }
        const result: [number, number][] = [];
        const checked = new Set<string>();
        for (let i = 0; i < positions.length; i++) {
            const candidates = qt.queryRange(positions[i], radii[i]);
            for (const j of candidates) {
                if (i < j) {
                    const key = `${i},${j}`;
                    if (checked.has(key)) continue;
                    checked.add(key);
                    if (this.circleCircle(positions[i], radii[i], positions[j], radii[j])) {
                        result.push([i, j]);
                    }
                }
            }
        }
        return result;
    }
    // ===== 3D 静态方法 =====
    /**
     * 3D球体碰撞检测
     * @param a 球A中心
     * @param r1 球A半径
     * @param b 球B中心
     * @param r2 球B半径
     * @returns 是否碰撞
     */
    static sphereSphere(a: Vector3, r1: number, b: Vector3, r2: number): boolean {
        const distSq = a.distanceSq(b);
        const r = r1 + r2;
        return distSq <= r * r;
    }
    /**
     * 3D盒体（AABB）碰撞检测
     * @param a 盒A中心
     * @param asize A尺寸
     * @param b 盒B中心
     * @param bsize B尺寸
     * @returns 是否碰撞
     */
    static boxBox(a: Vector3, asize: Vector3, b: Vector3, bsize: Vector3): boolean {
        return Math.abs(a.x - b.x) * 2 < (asize.x + bsize.x) &&
               Math.abs(a.y - b.y) * 2 < (asize.y + bsize.y) &&
               Math.abs(a.z - b.z) * 2 < (asize.z + bsize.z);
    }
    /**
     * 3D批量球体碰撞检测
     * @param positions 所有球心
     * @param radii 所有半径
     * @returns 所有碰撞对下标
     */
    static detectAllSphereCollisions(positions: Vector3[], radii: number[]): [number, number][] {
        const result: [number, number][] = [];
        for (let i = 0; i < positions.length; i++) {
            for (let j = i + 1; j < positions.length; j++) {
                if (this.sphereSphere(positions[i], radii[i], positions[j], radii[j])) {
                    result.push([i, j]);
                }
            }
        }
        return result;
    }
    /**
     * 3D八叉树加速批量球体碰撞检测
     * @param positions 所有球心
     * @param radii 所有半径
     * @param bounds 八叉树空间边界
     * @param maxObjects 单节点最大对象数
     * @param maxDepth 最大深度
     * @returns 所有碰撞对下标
     */
    static detectAllSphereCollisionsOctree(
        positions: Vector3[],
        radii: number[],
        bounds: { x: number, y: number, z: number, size: number },
        maxObjects = 8,
        maxDepth = 6
    ): [number, number][] {
        const octree = new Octree(new Vector3(bounds.x, bounds.y, bounds.z), bounds.size, maxObjects, maxDepth);
        for (let i = 0; i < positions.length; i++) {
            octree.insert({ position: positions[i], radius: radii[i], id: i });
        }
        const result: [number, number][] = [];
        const checked = new Set<string>();
        for (let i = 0; i < positions.length; i++) {
            const candidates = octree.queryRange(positions[i], radii[i]);
            for (const j of candidates) {
                if (i < j) {
                    const key = `${i},${j}`;
                    if (checked.has(key)) continue;
                    checked.add(key);
                    if (this.sphereSphere(positions[i], radii[i], positions[j], radii[j])) {
                        result.push([i, j]);
                    }
                }
            }
        }
        return result;
    }
    /**
     * 3D三角形碰撞检测
     * @param a 三角形A顶点
     * @param b 三角形B顶点
     * @returns 是否碰撞
     */
    static triangleTriangle(a: [Vector3, Vector3, Vector3], b: [Vector3, Vector3, Vector3]): boolean {
        const aabbA = AABB3D.fromPoints(a);
        const aabbB = AABB3D.fromPoints(b);
        if (!AABB3D.intersects(aabbA, aabbB)) return false;
        function getNormal(tri: [Vector3, Vector3, Vector3]): Vector3 {
            const e1 = tri[1].sub(tri[0]);
            const e2 = tri[2].sub(tri[0]);
            return e1.cross(e2).normalize();
        }
        function project(tri: [Vector3, Vector3, Vector3], axis: Vector3): [number, number] {
            let min = tri[0].dot(axis), max = min;
            for (let i = 1; i < 3; i++) {
                const p = tri[i].dot(axis);
                if (p < min) min = p;
                if (p > max) max = p;
            }
            return [min, max];
        }
        const axes: Vector3[] = [];
        axes.push(getNormal(a));
        axes.push(getNormal(b));
        for (let i = 0; i < 3; i++) {
            const ea = a[(i + 1) % 3].sub(a[i]);
            for (let j = 0; j < 3; j++) {
                const eb = b[(j + 1) % 3].sub(b[j]);
                const axis = ea.cross(eb);
                if (axis.lengthSq() > 1e-8) axes.push(axis.normalize());
            }
        }
        for (const axis of axes) {
            const [minA, maxA] = project(a, axis);
            const [minB, maxB] = project(b, axis);
            if (maxA < minB || maxB < minA) return false;
        }
        return true;
    }
    /**
     * 3D多面体碰撞检测
     * @param a 多面体A
     * @param b 多面体B
     * @returns 是否碰撞
     */
    static polyhedronPolyhedron(a: {vertices: Vector3[], faces: number[][]}, b: {vertices: Vector3[], faces: number[][]}): boolean {
        const aabbA = AABB3D.fromPoints(a.vertices);
        const aabbB = AABB3D.fromPoints(b.vertices);
        if (!AABB3D.intersects(aabbA, aabbB)) return false;
        function getFaceNormals(poly: {vertices: Vector3[], faces: number[][]}): Vector3[] {
            const normals: Vector3[] = [];
            for (const face of poly.faces) {
                const v0 = poly.vertices[face[0]];
                const v1 = poly.vertices[face[1]];
                const v2 = poly.vertices[face[2]];
                const normal = v1.sub(v0).cross(v2.sub(v0)).normalize();
                normals.push(normal);
            }
            return normals;
        }
        function getEdges(poly: {vertices: Vector3[], faces: number[][]}): Vector3[] {
            const edges: Vector3[] = [];
            for (const face of poly.faces) {
                for (let i = 0; i < face.length; i++) {
                    const v0 = poly.vertices[face[i]];
                    const v1 = poly.vertices[face[(i + 1) % face.length]];
                    edges.push(v1.sub(v0));
                }
            }
            return edges;
        }
        function projectPoly(poly: {vertices: Vector3[], faces: number[][]}, axis: Vector3): [number, number] {
            let min = poly.vertices[0].dot(axis), max = min;
            for (let i = 1; i < poly.vertices.length; i++) {
                const p = poly.vertices[i].dot(axis);
                if (p < min) min = p;
                if (p > max) max = p;
            }
            return [min, max];
        }
        const axes: Vector3[] = [...getFaceNormals(a), ...getFaceNormals(b)];
        const edgesA = getEdges(a);
        const edgesB = getEdges(b);
        for (const ea of edgesA) {
            for (const eb of edgesB) {
                const axis = ea.cross(eb);
                if (axis.lengthSq() > 1e-8) axes.push(axis.normalize());
            }
        }
        for (const axis of axes) {
            const [minA, maxA] = projectPoly(a, axis);
            const [minB, maxB] = projectPoly(b, axis);
            if (maxA < minB || maxB < minA) return false;
        }
        return true;
    }
    static create(obj: PhysicsObject): cBody {
        // 简单对象池实现
        return new cBody(obj);
    }
    /**
     * 插入物理Body到碰撞管理系统（可扩展为全局管理或空间结构插入）
     * @param body 物理对象cBody
     * @remarks
     *  - 默认实现为空，实际项目可扩展为：
     *    1. 统一管理所有body，便于批量遍历/清理
     *    2. 插入到空间划分结构（如四叉树/八叉树）加速碰撞检测
     *    3. 结合物理世界管理器等自动注册/注销
     * @example
     *   const body = Collision.create(obj);
     *   Collision.insert(body);
     */
    static insert(body: cBody) {
        // 这里可扩展为全局body管理或空间结构插入
        // 示例：无操作
    }
}

// 3D多面体类型
export interface Polyhedron3D {
    vertices: Vector3[];
    faces: number[][];
} 